class pidController:
    def __init__(self):
        self.kp = 0.0
        self.ki = 0.0
        self.kd = 0.0
        self.err = [0.0, 0.0]
        self.Ic = 0.0

    def setPID(self, kp, ki, kd):
        self.kp = kp
        self.ki = ki
        self.kd = kd

    def clear(self):
        self.Ic = 0

    def nextOutput(self, errInput):
        self.err[1] = self.err[0]
        self.err[0] = errInput
        if abs(errInput) < 1e-8:
            self.clear()
        else:
            self.Ic += self.ki * self.err[0]
        output = self.kp * errInput + self.kd * \
            (self.err[0]-self.err[1]) + self.Ic
        return output
